#include <string.h>
#include <stdlib.h>
#include "fifo.h"

uint32_t fifo_space(fifo_t *fifo)
{
	return (fifo->size - fifo->in + fifo->out);
}

uint32_t fifo_cnt(fifo_t *fifo)
{
	return (fifo->in - fifo->out);
}

uint32_t fifo_put(fifo_t *fifo, const void *buf, uint32_t len)
{
	uint32_t l;
	unsigned int space = fifo_space(fifo);

	len = C_MIN(len, space);

	l = C_MIN(len, fifo->size - (fifo->in & (fifo->size - 1)));
	memcpy(fifo->buf + (fifo->in & (fifo->size - 1)), buf, l);
	memcpy((char *)fifo->buf, (char *)buf + l, len - l);

	fifo->in += len;
	return len;
}

uint32_t fifo_get(fifo_t *fifo, void *buf, uint32_t len)
{
	uint32_t l;
	unsigned int cnt = fifo_cnt(fifo);

	len = C_MIN(len, cnt);

	l = C_MIN(len, fifo->size - (fifo->out & (fifo->size - 1)));
	memcpy(buf, fifo->buf + (fifo->out & (fifo->size -1)), l);
	memcpy((char *)buf + l, fifo->buf, len - l);

	fifo->out += len;
	return len;
}

fifo_t *fifo_new(size_t nmemb, size_t size)
{
	fifo_t *fifo = (fifo_t *)malloc(sizeof(fifo_t));
	memset(fifo, 0, sizeof(fifo));
	
	if (fifo) {
		uint8_t *buf = (uint8_t *)malloc(nmemb * size);
		if (NULL == buf) {
			free(fifo);
			return NULL;
		}
		fifo->buf = buf;
		fifo->size = nmemb * size;
		fifo->in = 0;
		fifo->out = 0;
		c_mutex_create(&fifo->lock, "NULL");
	}
	return fifo;
}

int fifo_in(fifo_t *fifo, const void *buf, size_t size, uint32_t tm)
{
	tm = 0;
	if (NULL == fifo) {
		return -1;
	}

	if (fifo_space(fifo) < size) {
		return -1;
	}

	return fifo_put(fifo, buf, size);
}

int fifo_out(fifo_t *fifo, void *buf, size_t size, uint32_t tm)
{
	tm = 0;
	if (NULL == fifo) {
		return -1;
	}

	if (fifo_cnt(fifo)  > size) {
		return -1;
	}

	return fifo_get(fifo, buf, size);
}

void fifo_free(fifo_t *fifo)
{
	if (fifo) {
		if (fifo->buf) {
			free(fifo->buf);
			fifo->buf = NULL;
		}
		fifo->in = fifo->out = 0;
		fifo->size = 0;
		c_mutex_delete(&fifo->lock);
		free(fifo);
		fifo = NULL;
	}
}

int fifo_tlv_read(fifo_t *fifo, int *type, unsigned char *data, unsigned int *size)
{
	int i = 0;
	tlv_t tlv = { .t = 0, .l = 0};
	unsigned char *p = (unsigned char *)&tlv;
	unsigned int len = *size;

	*size = 0;
	
	if (fifo_cnt(fifo) < sizeof(tlv)) {
		return -1;
	}

	for (i = 0; i < sizeof(tlv); i++) {
		p[i] = fifo->buf[(fifo->out + i) & (fifo->size - 1)];
	}

	if (fifo_cnt(fifo) < (sizeof(tlv) + tlv.l)) {
		return -1;
	}

	if (tlv.l > len) {
		return -2;
	}

	fifo_get(fifo, (unsigned char *)&tlv, sizeof(tlv));
	*type = tlv.t;
	*size = fifo_get(fifo, (unsigned char *)data, tlv.l);

	return 0;
}

int fifo_tlv_can_read(fifo_t *fifo, int *type)
{
	int i = 0;
	tlv_t tlv = { .t = 0, .l = 0};
	unsigned char *p = (unsigned char *)&tlv;

	if (fifo_cnt(fifo) < sizeof(tlv)) {
		return -1;
	}

	for (i = 0; i < sizeof(tlv); i++) {
		p[i] = fifo->buf[(fifo->out + i) & (fifo->size - 1)];
	}

	if (fifo_cnt(fifo) < (sizeof(tlv) + tlv.l)) {
		return -1;
	}

	*type = tlv.t;
	return 0;
}

int fifo_tlv_can_read_len(fifo_t *fifo, int *type)
{
	int i = 0;
	tlv_t tlv = { .t = 0, .l = 0};
	unsigned char *p = (unsigned char *)&tlv;

	if (fifo_cnt(fifo) < sizeof(tlv)) {
		return -1;
	}

	for (i = 0; i < sizeof(tlv); i++) {
		p[i] = fifo->buf[(fifo->out + i) & (fifo->size - 1)];
	}

	if (fifo_cnt(fifo) < (sizeof(tlv) + tlv.l)) {
		return -1;
	}

	*type = tlv.t;
	return tlv.l;
}

int fifo_tlv_write(fifo_t *fifo, int type, unsigned char *data, unsigned int size)
{
	tlv_t tlv;
	int l = 0, len = 0;
	unsigned char *str = NULL;

	if (NULL == fifo) {
		return -1;
	}

	if (fifo_space(fifo) < (size + sizeof(tlv))) {
		return -2;
	}

	tlv.t = (int)type;
	tlv.l = size;

	l = 0;
	str = (unsigned char *)&tlv;
	len = sizeof(tlv);
	while (l < len) {
		l += fifo_put(fifo, str + l, len - l);
	}

	l = 0;
	str = data;
	len = size;
	while (l < len) {
		l += fifo_put(fifo, str + l, len - l);
	}

	return 0;
}

int fifo_tlv_write_lock(fifo_t *fifo, int type, unsigned char *data, unsigned int size)
{
	int ret = 0;
	
	if (fifo->lock == NULL) {
		return -1;
	}

	c_mutex_lock(&fifo->lock);

	ret = fifo_tlv_write(fifo, type, data, size);

	c_mutex_unlock(&fifo->lock);

	return ret;
}
